package com.hitqz.robot.biz.flow.component.agv;

import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.hitqz.robot.api.business.enums.NavEnum;
import com.hitqz.robot.api.business.model.dto.FlowContextDto;
import com.hitqz.robot.api.business.model.dto.FlowPatrolPreAndEndTicketsDto;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.api.common.entity.TicketsTask;
import com.hitqz.robot.biz.business.dispatch.DispatchService;
import com.hitqz.robot.biz.flow.config.FlowInterruptException;
import com.hitqz.robot.biz.flow.util.LiteFlowUtil;
import com.hitqz.robot.biz.service.RobotMapPosService;
import com.hitqz.robot.biz.service.RobotService;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.yomahub.liteflow.annotation.LiteflowComponent;
import com.yomahub.liteflow.core.NodeComponent;
import lombok.AllArgsConstructor;
import lombok.extern.slf4j.Slf4j;

import java.util.Optional;
import java.util.concurrent.TimeUnit;

/**
 * @Author: Wang Weiwei
 * @Description: TODO2
 * @DateTime: 2024/5/6 10:44
 **/
@Slf4j
@LiteflowComponent(value = "GoTargetTargetPosition", name = "[轮对/轮轴]前往站点-终点")
@AllArgsConstructor
public class GoTargetTargetPosition extends NodeComponent {
    //@Autowired
    //private RobotDriverService robotDriverService;
    private final RobotDriverService robotDriverService;

    private final RobotMapPosService robotMapPosService;

    private final RobotService robotService;

    private final DispatchService dispatchService;
    @Override
    public void process() throws Exception {
        LiteFlowUtil.checkInterruptFlag();
        String posName = null;
        FlowContextDto dto =  this.getRequestData();
        TicketsTask task = dto.getTicketsTask();
        Robot robot = robotService.getDefaultRobot();
        FlowPatrolPreAndEndTicketsDto ticketsDto = JSONObject.parseObject(dto.getTicketsTask()
                .getParseOrderParams(), FlowPatrolPreAndEndTicketsDto.class);
        //String targetName = ticketsDto.getModalData().get("targetPosName")== null ?
        //        String.valueOf(ticketsDto.getModalData().get("targetPosCode")):
        //        String.valueOf(ticketsDto.getModalData().get("targetPosName"));
        //String sourceName = ticketsDto.getModalData().get("sourcePosName")== null ?
        //        String.valueOf(ticketsDto.getModalData().get("sourcePosCode")):
        //        String.valueOf(ticketsDto.getModalData().get("sourcePosName"));
        String sourceName = Optional.ofNullable(ticketsDto.getModalData().get("sourcePosName"))
                .orElse(ticketsDto.getModalData().get("sourcePosCode")).toString();
        String targetName = Optional.ofNullable(ticketsDto.getModalData().get("targetPosName"))
                .orElse(ticketsDto.getModalData().get("targetPosCode")).toString();

        //log.info("{} ---params: {}", task.getParseOrderParams(), JSONObject.toJSONString(params));
        RobotMapPos robotMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                .eq(RobotMapPos::getName, targetName)
                .eq(RobotMapPos::getMapCode, robot.getMapCode()));
        if (robotMapPos == null) {
            robotMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                    .eq(RobotMapPos::getCode, targetName)
                    .eq(RobotMapPos::getMapCode, robot.getMapCode()));
        }
        if (targetName.equals("911012")) {
            String reName = targetName + "-" + ticketsDto.getModalData().get("srcnum");
            if (sourceName.contains("LM18")) {
                reName = targetName + "-4";
            }
            log.info("上线区--终点" + reName);
            robotMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                    .eq(RobotMapPos::getName, reName)
                    .eq(RobotMapPos::getMapCode, robot.getMapCode()));
        }

        log.info("前往终点目标点{}", robotMapPos.getCode());

        boolean isSuccess = dispatchService.go2TargetPosition(robotMapPos, false);
        //robotDriverService.robotTaskGoTarget(
        //        RobotGoTargetPosDto.builder()
        //                .robot(robotService.getDefaultRobot())
        //                .code(robotMapPos.getCode())
        //                .charge(false).posId(robotMapPos.getId().intValue())
        //                .taskId(UUID.randomUUID().toString().replace("-","")).build()
        //);
        TimeUnit.SECONDS.sleep(3);
        IRobotPosDto iRobotPosDto = robotDriverService.getWebPos();
        //while (iRobotPosDto == null || (StrUtil.isNotBlank(iRobotPosDto.getCurrentStation()) &&
        //        !iRobotPosDto.getCurrentStation().equals(robotMapPos.getCode())) || (!iRobotPosDto.isReached())) {
        //    iRobotPosDto = robotDriverService.getWebPos();
        //}
        while (true) {
            try {
                iRobotPosDto = robotDriverService.getWebPos();
                if (iRobotPosDto != null &&
                        iRobotPosDto.getCurrentStation() != null &&
                        robotMapPos.getCode().equals(iRobotPosDto.getCurrentStation())
                        && iRobotPosDto.isReached()) {
                    log.info("机器人到达位置");
                    break;
                }
                if (iRobotPosDto.getTaskStatus() == NavEnum.SUSPENDED.getCode()) {
                    log.info("导航暂停,继续下发");
                    dispatchService.go2TargetPosition(robotMapPos, false);
                }
                LiteFlowUtil.checkInterruptFlag();
                TimeUnit.SECONDS.sleep(1);
            } catch (FlowInterruptException e) {
                break;
            }
        }
        log.info("机器人到达位置了");
        //log.info("pre" + JSONObject.toJSONString(iRobotPosDto));
        //TimeUnit.SECONDS.sleep(2);
        iRobotPosDto = robotDriverService.getWebPos();
        log.info("到达点位激光坐标 {}---- x: {}, y: {}, yaw {}", iRobotPosDto.getCurrentStation(),
                iRobotPosDto.getX(), iRobotPosDto.getY(), iRobotPosDto.getAngle()
        );
    }
}